#ifndef OMPL_BASE_OCTOMAP_COST_ESTIMATOR_
#define OMPL_BASE_OCTOMAP_COST_ESTIMATOR_

#include "ompl_extension/CostEstimatorBase.h"

#include <planning_environment/models/collision_models_interface.h>
#include <octomap/OcTreeStamped.h>
#include <octomap_ros/OctomapROS.h>
#include <octomap_ros/OctomapBinary.h>
#include <octomap_ros/GetOctomap.h>
#include <ompl_ros_interface_helper/ompl_ros_conversions.h>

namespace ompl
{
  namespace base
  {
    void worldToGrid(const btVector3& origin, double wx, double wy, double wz, double resolution, int& gx, int& gy, int& gz);

    void gridToWorld(const btVector3& origin, int gx, int gy, int gz, double resolution, double& wx, double& wy, double& wz);

    void getVoxelsInBody(const bodies::Body& body, double resolution, std::vector<btVector3>& voxels);

    class OctomapStateCostEstimator : public ompl::base::StateCostEstimator
    {
    public:
      typedef octomap::OcTreeStamped OctreeType;

      OctomapStateCostEstimator(ompl::base::SpaceInformation* si,
                              planning_environment::CollisionModelsInterface* cmi,
                              const std::string& group_name,
                              const ros::NodeHandle& node_handle,
                              double resolution);

      double computeCost(const ompl::base::State* state) const;

      ros::NodeHandle node_handle_;

      OctreeType octree_;

      planning_environment::CollisionModelsInterface* cmi_;

      const planning_models::KinematicModel::JointModelGroup* joint_group_;

      std::vector<const planning_models::KinematicModel::LinkModel*> group_links_;

      ompl_ros_interface_helper::OmplStateToKinematicStateMapping ompl_state_to_kinematic_state_mapping_;
      ompl_ros_interface_helper::KinematicStateToOmplStateMapping kinematic_state_to_ompl_state_mapping_;

      std::map<const planning_models::KinematicModel::LinkModel*, std::vector<btVector3> > link_samples_;

      double resolution_;
    };

    class OctomapMotionCostEstimator : public ompl::base::MotionCostEstimator
    {
    public:
      typedef octomap::OcTreeStamped OctreeType;

      OctomapMotionCostEstimator(ompl::base::SpaceInformation* si,
                                 planning_environment::CollisionModelsInterface* cmi,
                                 const std::string& group_name,
                                 const ros::NodeHandle& node_handle,
                                 double resolution,
                                 unsigned int interpolate_num = 2);

      /** \brief the cost is simple now: length * (1 + collision_prob) */
      double computeCost(const ompl::base::State* s1, const ompl::base::State* s2) const;

      ros::NodeHandle node_handle_;

      OctreeType octree_;

      planning_environment::CollisionModelsInterface* cmi_;

      const planning_models::KinematicModel::JointModelGroup* joint_group_;

      std::vector<const planning_models::KinematicModel::LinkModel*> group_links_;

      ompl_ros_interface_helper::OmplStateToKinematicStateMapping ompl_state_to_kinematic_state_mapping_;
      ompl_ros_interface_helper::KinematicStateToOmplStateMapping kinematic_state_to_ompl_state_mapping_;

      std::map<const planning_models::KinematicModel::LinkModel*, std::vector<btVector3> > link_samples_;

      double resolution_;

      unsigned int interpolate_num_;
    };
  }
}


#endif
